#include <stdio.h>
#include <stdlib.h>
#include <memory.h>
#include <string.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "ssd1306.h"
#include "iot_i2c.h"
#include "iot_watchdog.h"
#include "robot_control.h"
#include "iot_errno.h"
#include "oled_display.h"

#define OLED_I2C_BAUDRATE 400*1000
#define GPIO13 13
#define GPIO14 14
#define FUNC_SDA 6
#define FUNC_SCL 6
extern unsigned char g_car_status;
extern unsigned char get_motor_speed_level(void);
extern unsigned char get_current_speed_level(void);
extern unsigned char g_actual_speed;
void Ssd1306TestTask(void* arg)
{
    (void) arg;
    hi_io_set_func(GPIO13, FUNC_SDA);
    hi_io_set_func(GPIO14, FUNC_SCL);
    IoTI2cInit(0, OLED_I2C_BAUDRATE);

    IoTWatchDogDisable();

    usleep(20*1000);
    ssd1306_Init();

    // ?????OLED???
    oled_display_init();
    osDelay(2000); // ??????????2??

    // Status change detection variables
    static unsigned char last_car_status = 255;  // Initialize to invalid value
    static unsigned char last_speed_level = 255; // Initialize to invalid value

    while (1)
    {
        // Get current status
        unsigned char current_car_status = g_car_status;
        unsigned char current_speed_level;

        // 在远程控制模式下，使用速度等级而不是PWM值
        if (current_car_status == 3) 
        { // CAR_REMOTE_CONTROL_STATUS
            extern unsigned char g_current_level_index;
            current_speed_level = g_actual_speed; // 使用速度等级(0-5)
        } 
        else 
        {
            current_speed_level = get_motor_speed_level(); // 其他模式使用PWM值
        }

        // Detect mode change
        if (current_car_status != last_car_status && last_car_status != 255) {
            oled_display_mode_change(current_car_status);
            osDelay(500); // Show mode change for 0.5 seconds
        }

        // Detect speed change
        if (current_speed_level != last_speed_level && last_speed_level != 255) {
            oled_display_speed_change(last_speed_level, current_speed_level);
            osDelay(500); // Show speed change for 0.5 seconds
        }

        // Display normal status interface
        oled_display_car_status(current_car_status, current_speed_level);

        // Update last status
        last_car_status = current_car_status;
        last_speed_level = current_speed_level;

        // Debug output - 显示不同模式下的速度信息
        if (current_car_status == 3) {
            printf("Car Status: %d (Remote), Speed Level: %d\r\n", current_car_status, current_speed_level);
        } else {
            printf("Car Status: %d, Speed: %d%%\r\n", current_car_status, current_speed_level);
        }

        // Delay to reduce refresh frequency
        osDelay(100);
    }
    
}

void Ssd1306TestDemo(void)
{
    osThreadAttr_t attr;

    attr.name = "Ssd1306Task";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 10240;
    attr.priority = 25;

    if (osThreadNew(Ssd1306TestTask, NULL, &attr) == NULL) {
        printf("[Ssd1306TestDemo] Falied to create Ssd1306TestTask!\n");
    }
}
APP_FEATURE_INIT(Ssd1306TestDemo);
